{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# ScenarioRunner\n",
    "\n",
    "<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/navigation/doc/ScenarioRunner.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>\n",
    "\n",
    "## Overview\n",
    "\n",
    "The `ScenarioRunner` class is a utility designed for testing IMU preintegration and factors. It takes a ground truth trajectory defined by a `Scenario` object, along with IMU parameters (`PreintegrationParams` or `PreintegrationCombinedParams`) and a specified IMU bias, and simulates the measurements an IMU would produce while following that trajectory.\n",
    "\n",
    "Key capabilities include:\n",
    "- Calculating the ideal specific force and angular velocity at any time `t` based on the scenario's motion.\n",
    "- Generating noisy IMU measurements by adding the specified bias and sampling from the noise models defined in the parameters.\n",
    "- Integrating these simulated measurements over a time interval `T` to produce a `PreintegratedImuMeasurements` or `PreintegratedCombinedMeasurements` object.\n",
    "- Predicting the final state based on an initial state and a preintegrated measurement object.\n",
    "- Estimating the covariance of the preintegrated measurements or the prediction error via Monte Carlo simulation (useful for verifying the analytical covariance propagation)."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "tags": [
     "remove-cell"
    ]
   },
   "outputs": [],
   "source": [
    "%pip install --quiet gtsam-develop"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Measurement Simulation\n",
    "\n",
    "- **`actualAngularVelocity(t)`**: Returns the true angular velocity $\\omega_b(t)$ from the `Scenario`.\n",
    "- **`actualSpecificForce(t)`**: Calculates the true specific force (what an ideal accelerometer measures) in the body frame. This is the body-frame acceleration $a_b(t)$ *minus* the body-frame representation of gravity $g_b(t)$:\n",
    "  $$ f_b(t) = a_b(t) - R_{bn}(t) g_n = R_{bn}(t) (a_n(t) - g_n) $$ \n",
    "  where $g_n$ is the gravity vector defined in `PreintegrationParams`.\n",
    "- **`measuredAngularVelocity(t)`**: Adds bias and sampled noise to `actualAngularVelocity(t)`.\n",
    "  $$ \\omega_{measured} = \\omega_b(t) + b_g + \\eta_{gd} $$ \n",
    "  where $b_g$ is the gyro bias and $\\eta_{gd}$ is sampled discrete gyro noise.\n",
    "- **`measuredSpecificForce(t)`**: Adds bias and sampled noise to `actualSpecificForce(t)`.\n",
    "  $$ a_{measured} = f_b(t) + b_a + \\eta_{ad} $$ \n",
    "  where $b_a$ is the accelerometer bias and $\\eta_{ad}$ is sampled discrete accelerometer noise."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Key Functionality / API\n",
    "\n",
    "- **Constructor**: `ScenarioRunner(scenario, params, imuSampleTime=0.01, bias=ConstantBias())`: Takes the scenario, IMU parameters, sample rate, and the *true* bias to apply to measurements.\n",
    "- **Measurement Methods**: `actual...()`, `measured...()` as described above.\n",
    "- **`integrate(T, estimatedBias, corrupted=True)`**: Simulates measurements at `imuSampleTime` intervals for a duration `T`, optionally adding noise (`corrupted=True`), and integrates them into a PIM object using `estimatedBias` as the `biasHat` for the PIM.\n",
    "- **`predict(pim, estimatedBias)`**: Uses the provided PIM (which contains $\\Delta R, \\Delta p, \\Delta v$) and an `estimatedBias` to predict the final `NavState` starting from the scenario's initial state at $t=0$.\n",
    "- **`estimateCovariance(T, N, estimatedBias)`**: Performs Monte Carlo simulation: runs `integrate` `N` times with different noise samples, calculates the `predict`ed state for each, and computes the sample covariance of the 9D tangent space difference between the Monte Carlo predictions and the mean prediction. Used to verify `pim.preintMeasCov()`.\n",
    "- **`estimateNoiseCovariance(N)`**: Samples noise `N` times and computes the sample covariance, to verify the noise samplers themselves.\n",
    "\n",
    "There is also a `CombinedScenarioRunner` inheriting from `ScenarioRunner` that works with `PreintegrationCombinedParams` and `PreintegratedCombinedMeasurements`."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Usage Example\n",
    "\n",
    "Using the `AcceleratingScenario` from the `Scenario` example to generate measurements and verify prediction."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "--- Integration Results (T=2.0s) ---\n",
      "Bias Hat used in PIM:\n",
      "acc =  0.04 -0.04  0.09 gyro =  0.005 -0.005  0.015\n",
      "PIM Delta R:\n",
      "R: [\n",
      "\t0.9998, -0.00700878, -0.0187374;\n",
      "\t0.00661627, 0.999759, -0.0209287;\n",
      "\t0.0188796, 0.0208006, 0.999605\n",
      "]\n",
      "PIM Delta P: [ 0.70090612 -0.06192379 19.6232812 ]\n",
      "PIM Delta V: [ 0.62536677 -0.17585342 19.47976488]\n",
      "\n",
      "--- Prediction vs Ground Truth (T=2.0s) ---\n",
      "Predicted State:\n",
      "R: [\n",
      "\t0.9998, -0.00700878, -0.0187374;\n",
      "\t0.00661627, 0.999759, -0.0209287;\n",
      "\t0.0188796, 0.0208006, 0.999605\n",
      "]\n",
      "p:   0.700906 -0.0619238  0.0032812\n",
      "v:  0.625367 -0.175853 -0.140235\n",
      "\n",
      "Ground Truth State:\n",
      "R: [\n",
      "\t1, 0, 0;\n",
      "\t0, 1, 0;\n",
      "\t0, 0, 1\n",
      "]\n",
      "p: 1 0 0\n",
      "v: 1 0 0\n",
      "\n",
      "Prediction Error (Logmap(predicted^-1 * ground_truth)):\n",
      " [-0.02086757  0.01881111 -0.00681348  0.29938178  0.05974434 -0.01018013\n",
      "  0.37836933  0.1761023   0.12947973]\n"
     ]
    }
   ],
   "source": [
    "import gtsam\n",
    "import numpy as np\n",
    "\n",
    "# --- 1. Define Scenario --- \n",
    "initial_pose_accel = gtsam.Pose3() \n",
    "initial_vel_n = np.array([0.0, 0.0, 0.0])\n",
    "const_accel_n = np.array([0.5, 0.0, 0.0]) # Accelerate along nav X (ENU)\n",
    "const_omega_b = np.array([0.0, 0.0, 0.0]) # No rotation\n",
    "scenario = gtsam.AcceleratingScenario(\n",
    "    initial_pose_accel.rotation(), initial_pose_accel.translation(),\n",
    "    initial_vel_n, const_accel_n, const_omega_b\n",
    ")\n",
    "\n",
    "# --- 2. Define IMU Params and Runner --- \n",
    "# Use default ENU parameters (Z-up, g=-9.81)\n",
    "params = gtsam.PreintegrationParams.MakeSharedU(9.81)\n",
    "# Add some noise (variances)\n",
    "params.setAccelerometerCovariance(np.eye(3) * 0.01)\n",
    "params.setGyroscopeCovariance(np.eye(3) * 0.0001)\n",
    "params.setIntegrationCovariance(np.eye(3) * 1e-9)\n",
    "\n",
    "imu_sample_dt = 0.01 # 100 Hz\n",
    "# Define the *true* bias applied to the measurements\n",
    "true_bias = gtsam.imuBias.ConstantBias(np.array([0.05, -0.05, 0.1]), \n",
    "                                       np.array([0.01, -0.01, 0.02]))\n",
    "\n",
    "runner = gtsam.ScenarioRunner(scenario, params, imu_sample_dt, true_bias)\n",
    "\n",
    "# --- 3. Integrate Measurements --- \n",
    "integration_time = 2.0 # seconds\n",
    "\n",
    "# Define the bias *estimate* to be used during preintegration\n",
    "# Let's assume we have a slightly wrong estimate\n",
    "estimated_bias = gtsam.imuBias.ConstantBias(np.array([0.04, -0.04, 0.09]), \n",
    "                                            np.array([0.005, -0.005, 0.015]))\n",
    "\n",
    "# Integrate noisy measurements using the 'estimated_bias' as biasHat for the PIM\n",
    "pim = runner.integrate(integration_time, estimatedBias=estimated_bias, corrupted=True)\n",
    "\n",
    "print(f\"--- Integration Results (T={integration_time}s) ---\")\n",
    "print(\"Bias Hat used in PIM:\")\n",
    "pim.biasHat().print()\n",
    "print(\"PIM Delta R:\")\n",
    "pim.deltaRij().print()\n",
    "print(\"PIM Delta P:\", pim.deltaPij())\n",
    "print(\"PIM Delta V:\", pim.deltaVij())\n",
    "\n",
    "# --- 4. Predict State --- \n",
    "# Predict the state at integration_time using the PIM and the *same* estimated_bias\n",
    "initial_state_actual = scenario.navState(0.0)\n",
    "predicted_state = runner.predict(pim, estimated_bias)\n",
    "\n",
    "# Get the ground truth state at the end time\n",
    "ground_truth_state = scenario.navState(integration_time)\n",
    "\n",
    "print(f\"\\n--- Prediction vs Ground Truth (T={integration_time}s) ---\")\n",
    "print(\"Predicted State:\")\n",
    "predicted_state.print()\n",
    "print(\"\\nGround Truth State:\")\n",
    "ground_truth_state.print()\n",
    "\n",
    "# Calculate the error (difference in tangent space)\n",
    "prediction_error = predicted_state.localCoordinates(ground_truth_state)\n",
    "print(\"\\nPrediction Error (Logmap(predicted^-1 * ground_truth)):\\n\", prediction_error)\n",
    "# Note: Error is non-zero due to noise and bias estimation error used in PIM.\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Source\n",
    "- [ScenarioRunner.h](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/ScenarioRunner.h)\n",
    "- [ScenarioRunner.cpp](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/ScenarioRunner.cpp)"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "py312",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.12.6"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
